#ifndef _UDP_SENDPOINTS_HPP
#define _UDP_SENDPOINTS_HPP

#include <arpa/inet.h>
#include <fcntl.h>
#include <ifaddrs.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <netinet/in.h>
#include <poll.h>
#include <pthread.h>
#include <sched.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>

#include <iostream>
#include <opencv2/opencv.hpp>
#include <queue>

#include "common.h"

using namespace cv;
using namespace std;

#define SERVER_PORT 2022

class CUdpSendPoints {
 public:
  CUdpSendPoints();
  ~CUdpSendPoints();
  void SetAddr(const char **addr, int port);
  void UdpMsgSender(const vector<Point2f> &pts, int idx);
  void DivideGroup(const vector<Point2f> &pts, int part_num);
  void SendPoints(const vector<Point2f> &pts, int idx);
  void UpdateParams(const int img_width, const int img_height, const int target_height, const int target_side_lenth);
  void SendRawPoints(const vector<Point2f> &pts);
  void SendH(const Mat &h_mat);

 private:
  const int mDstNum = 5;
  vector<int> mClientFdVec;
  vector<sockaddr_in> mAddressVec;
  char mSendBuf[19];
  bool IsInRange(const Point2f &src, const Point2f &bound_min, const Point2f &bound_max);
  bool ShouldSend(const Point2f &src, const Point2f &origin, Point2f &send_pt);
  double mWidth = 1920.0;
  double mHeight = 1080.0;
  double mTargetHeight = 400.0;
  double mTargetSideLength = 250.0;
  double mRealTargetSideLength = 500.0;
  double mScaleUp = 2.0;
  vector<vector<Point2f>> mTargetOrigins;
};

CUdpSendPoints::CUdpSendPoints() {
  // Create sockets
  int client_fd;
  for (int i = 0; i < mDstNum; ++i) {
    client_fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (client_fd < 0) {
      printf("create socket fail!\n");
    }
    mClientFdVec.push_back(client_fd);
  }
  UpdateParams(mWidth, mHeight, mTargetHeight, mTargetSideLength);
}

CUdpSendPoints::~CUdpSendPoints() {
  // Close sockets
  for (const int &client_fd : mClientFdVec) {
    close(client_fd);
  }
}

void CUdpSendPoints::UpdateParams(const int img_width, const int img_height, const int target_height,
                                  const int target_side_lenth) {
  mWidth = img_width;
  mHeight = img_height;
  mTargetHeight = target_height;
  mTargetSideLength = target_side_lenth;
  mScaleUp = mRealTargetSideLength / mTargetSideLength;

  double origin_y = mTargetHeight;
  double origin_x = 0;
  for (int row = 1; row <= mDstNum; ++row) {
    vector<Point2f> row_pts;
    double x_step = (double)mWidth / row;
    for (int col = 0; col < row; ++col) {
      origin_x = x_step * (col + 0.5) - mTargetSideLength * 0.5;
      row_pts.push_back(Point2f(origin_x, origin_y));
    }
    mTargetOrigins.push_back(row_pts);
  }
}

void CUdpSendPoints::SetAddr(const char **addr, int port) {
  struct sockaddr_in ser_address;
  for (int i = 0; i < mDstNum; ++i) {
    memset(&ser_address, 0, sizeof(ser_address));
    ser_address.sin_family = AF_INET;
    ser_address.sin_addr.s_addr = inet_addr(addr[i]);
    ser_address.sin_port = htons(port);  // 注意网络序转换
    mAddressVec.push_back(ser_address);
  }
}

bool CUdpSendPoints::IsInRange(const Point2f &src, const Point2f &bound_min, const Point2f &bound_max) {
  return !(src.x < bound_min.x || src.x >= bound_max.x || src.y < bound_min.y || src.y >= bound_max.y);
}

bool CUdpSendPoints::ShouldSend(const Point2f &src, const Point2f &origin, Point2f &send_pt) {
  send_pt.x = (src.x - origin.x);
  send_pt.y = (src.y - origin.y);
  return IsInRange(send_pt, Point2f(0, 0), Point2f(mTargetSideLength, mTargetSideLength));
}

void CUdpSendPoints::DivideGroup(const vector<Point2f> &pts, int part_num) {
  Point2f bound_min = Point2f(0, 0);
  Point2f bound_max = Point2f(mWidth, mHeight);
  Point2f send_pt = Point2f(0, 0);
  vector<vector<Point2f>> pts_group;
  pts_group.resize(part_num);

  for (const Point2f &pt : pts) {
    if (!IsInRange(pt, bound_min, bound_max)) continue;
    for (int col = 0; col < part_num; ++col) {
      if (ShouldSend(pt, mTargetOrigins[part_num - 1][col], send_pt)) {
        send_pt.x = send_pt.x * mScaleUp;
        send_pt.y = send_pt.y * mScaleUp;
        pts_group[col].push_back(send_pt);
      }
    }
  }

  // switch (part_num)
  // {
  // case 1:
  //   for (const Point2f &pt : pts)
  //   {
  //     if (!IsInRange(pt, bound_min, bound_max))
  //       continue;
  //     if (ShouldSend(pt, Point2f(835, 400), send_pt))
  //     {
  //       pts_group[0].push_back(send_pt);
  //     }
  //   }
  //   break;
  // case 2:
  //   for (const Point2f &pt : pts)
  //   {
  //     if (!IsInRange(pt, bound_min, bound_max))
  //       continue;
  //     if (ShouldSend(pt, Point2f(355, 400), send_pt))
  //       pts_group[0].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(1315, 400), send_pt))
  //       pts_group[1].push_back(send_pt);
  //   }
  //   break;
  // case 3:
  //   for (const Point2f &pt : pts)
  //   {
  //     if (!IsInRange(pt, bound_min, bound_max))
  //       continue;
  //     if (ShouldSend(pt, Point2f(195, 400), send_pt))
  //       pts_group[0].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(835, 400), send_pt))
  //       pts_group[1].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(1475, 400), send_pt))
  //       pts_group[2].push_back(send_pt);
  //   }
  //   break;
  // case 4:
  //   for (const Point2f &pt : pts)
  //   {
  //     if (!IsInRange(pt, bound_min, bound_max))
  //       continue;
  //     if (ShouldSend(pt, Point2f(115, 400), send_pt))
  //       pts_group[0].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(595, 400), send_pt))
  //       pts_group[1].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(1075, 400), send_pt))
  //       pts_group[2].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(1555, 400), send_pt))
  //       pts_group[3].push_back(send_pt);
  //   }
  //   break;
  // case 5:
  //   for (const Point2f &pt : pts)
  //   {
  //     if (!IsInRange(pt, bound_min, bound_max))
  //       continue;
  //     if (ShouldSend(pt, Point2f(67, 400), send_pt))
  //       pts_group[0].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(451, 400), send_pt))
  //       pts_group[1].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(835, 400), send_pt))
  //       pts_group[2].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(1219, 400), send_pt))
  //       pts_group[3].push_back(send_pt);
  //     if (ShouldSend(pt, Point2f(1603, 400), send_pt))
  //       pts_group[4].push_back(send_pt);
  //   }
  //   break;
  // default:
  //   break;
  // }

  for (int i = 0; i < part_num; ++i) {
    if (pts_group[i].size() > 0) {
      UdpMsgSender(pts_group[i], i);
    }
  }
}

void CUdpSendPoints::UdpMsgSender(const vector<Point2f> &pts, int idx) {
  int len = pts.size();
  int step = 3, start = 0, end = 0;
  while (len > start) {
    end = min(start + step, len);
    vector<Point2f> tmp(pts.begin() + start, pts.begin() + end);
    SendPoints(tmp, idx);
    start += step;
  }
}

void CUdpSendPoints::SendPoints(const vector<Point2f> &pts, int idx) {
  char SendBuf[35];
  memset(SendBuf, 0, sizeof(SendBuf));
  SendBuf[0] = 0x55;
  SendBuf[1] = 0xaa;
  char low, high;
  for (int i = 0; i < 3 && i < pts.size(); ++i) {
    low = (char)((int)(pts[i].x) & 0xff);
    high = (char)(((int)(pts[i].x) & 0xff00) >> 8);
    SendBuf[i * 4 + 2] = low;
    SendBuf[i * 4 + 3] = high;
    low = (char)((int)(pts[i].y) & 0xff);
    high = (char)(((int)(pts[i].y) & 0xff00) >> 8);
    SendBuf[i * 4 + 4] = low;
    SendBuf[i * 4 + 5] = high;
  }
  SendBuf[30] = 0x00;
  SendBuf[31] = 0x00;
  SendBuf[32] = 0x00;
  SendBuf[33] = 0x00;
  SendBuf[34] = 0xa0;
  // Use UDP to send points
  struct sockaddr *udp_dst;
  socklen_t udp_dst_len;
  udp_dst = (struct sockaddr *)&(mAddressVec[idx]);
  udp_dst_len = sizeof(*udp_dst);
  int client_fd = mClientFdVec[idx];
  sendto(client_fd, SendBuf, sizeof(SendBuf), 0, udp_dst, udp_dst_len);
}

void CUdpSendPoints::SendRawPoints(const vector<Point2f> &pts) {
  char send_buf[39];
  send_buf[0] = 0x55;
  send_buf[1] = 0xaa;
  char low, high;
  int num_pts = min(8, static_cast<int>(pts.size()));
  for (int i = 0; i < num_pts; ++i) {
    low = (char)((int)(pts[i].x * 100) & 0xff);
    high = (char)(((int)(pts[i].x * 100) & 0xff00) >> 8);
    send_buf[i * 4 + 2] = low;
    send_buf[i * 4 + 3] = high;
    low = (char)((int)(pts[i].y * 100) & 0xff);
    high = (char)(((int)(pts[i].y * 100) & 0xff00) >> 8);
    send_buf[i * 4 + 4] = low;
    send_buf[i * 4 + 5] = high;
  }
  send_buf[34] = 0x00;
  send_buf[35] = 0x00;
  send_buf[36] = 0x00;
  send_buf[37] = 0x00;
  send_buf[38] = 0xd0;
  // Use UDP to send points
  struct sockaddr *udp_dst;
  socklen_t udp_dst_len;
  udp_dst = (struct sockaddr *)&(mAddressVec[0]);
  udp_dst_len = sizeof(*udp_dst);
  int client_fd = mClientFdVec[0];

  sendto(client_fd, send_buf, sizeof(send_buf), 0, udp_dst, udp_dst_len);
}

void CUdpSendPoints::SendH(const Mat &h_mat) {
  if (h_mat.rows != 3 || h_mat.cols != 3) {
    std::cout << "Error H.size: " << h_mat.rows << "," << h_mat.cols << std::endl;
    return;
  }
  char send_buf[39];
  send_buf[0] = 0x55;
  send_buf[1] = 0xaa;
  send_buf[38] = 0xa0;
  int count = 0;
  cout << "SendH:" << endl;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      float tmp_element = (float)(h_mat.at<double>(i, j));
      cout << tmp_element << ",";
      std::memcpy(&send_buf[2 + count * 4], &tmp_element, sizeof(float));
      count++;
    }
  }
  cout << endl;

  // Use UDP to send points
  struct sockaddr *udp_dst;
  socklen_t udp_dst_len;
  udp_dst = (struct sockaddr *)&(mAddressVec[0]);
  udp_dst_len = sizeof(*udp_dst);
  int client_fd = mClientFdVec[0];

  sendto(client_fd, send_buf, sizeof(send_buf), 0, udp_dst, udp_dst_len);
}

#endif
